#include "odecontroller.h"
#include "transformable.h"
#include "scene.h"
#include <cassert>
#include <ode/odeinit.h>
#include "datatypedefine.h"

extern "C" void ODEMessageHandler(int errnum, const char* msg, va_list ap)
{
   //log
}

extern "C" void ODEDebugHandler(int errnum, const char* msg, va_list ap)
{
  //log
   assert(false);
}

extern "C" void ODEErrorHandler(int errnum, const char* msg, va_list ap)
{
  //log
   assert(false);
}
namespace CoreBase
{
   static OpenThreads::Atomic ODERefCount;

   static void RefODE()
   {
      if (++ODERefCount == 1)
      {
         if (!dInitODE2(0))
         {
            //log("ODE failed to initialize.");
         }
      }
   }
   static void DerefODE()
   {
      if (--ODERefCount == 0)
      {
         dCloseODE();
      }
   }

ODEController::ODEController(Base* msgSender):_spaceWrapper(NULL),
_worldWrapper(new ODEWorldWrap()),_physicsStepSize(0.0),_msgSender(msgSender)
{
   _spaceWrapper = new ODESpaceWrap(_worldWrapper.get());
   ctor();
}

ODEController::ODEController(ODESpaceWrap& spaceWrapper, ODEWorldWrap& worldWrap, Base* msgSender):
_spaceWrapper(&spaceWrapper),_worldWrapper(&worldWrap),_physicsStepSize(0.0),_msgSender(msgSender)
{
   ctor();
}

unsigned ODEController::getODERefCount()
{
   return ODERefCount;
}

void ODEController::ctor()
{
   RefODE();
   //supply our method to be called when geoms actually collide
   _spaceWrapper->setDefaultCollisionCBFunc(&ODEController::defaultCBFunc,this);
   dSetMessageHandler(ODEMessageHandler);
   dSetDebugHandler(ODEDebugHandler);
   dSetErrorHandler(ODEErrorHandler);
}

ODEController::~ODEController()
{
   DerefODE();
   // Since we are going to destroy all the bodies in our world with dWorldDestroy,
   // we must remove the references to the bodies associated with their default collision
   // geoms. Otherwise destroying the world will leave the geoms references bad memory.
   // This prevents a crash-on-exit in STAGE.
   for (QVector<Transformable*>::iterator iter = _collidableContents.begin();
        iter != _collidableContents.end();
        ++iter)
   {
      _spaceWrapper->unRegisterCollidable((*iter));
      _worldWrapper->unRegisterCollidable((*iter));
   }
   _collidableContents.clear();
}

void ODEController::iterate(double deltaFrameTime)
{
   double stepSize = deltaFrameTime;
   // if step size is set, use it instead of the delta frame time
   if (getPhysicsStepSize() > 0.0)
   {
      stepSize = getPhysicsStepSize();
   }
   //calc the number of steps to take
   const int numSteps = int(deltaFrameTime/stepSize);
   QVector<Transformable*>::const_iterator it;
   for (it = getRegisteredCollidables().begin();
        it != getRegisteredCollidables().end();
        ++it)
   {
      (*it)->prePhysicsStepUpdate();
   }
   for (int i=0; i<numSteps; ++i)
   {
      step(stepSize);
   }
   const double leftOver = deltaFrameTime - (numSteps * stepSize);
   if (leftOver > 0.0)
   {
      step(leftOver);
   }
   for (it = getRegisteredCollidables().begin();
        it != getRegisteredCollidables().end();
        ++it)
   {
      (*it)->postPhysicsStepUpdate();
   }
}

double ODEController::getPhysicsStepSize() const
{
   return _physicsStepSize;
}

void ODEController::setPhysicsStepSize(double stepSize)
{
   _physicsStepSize = stepSize;
}

void ODEController::registerCollidable(Transformable* collidable)
{
   if (collidable == NULL) {return;}
   _spaceWrapper->registerCollidable(collidable);
   _worldWrapper->registerCollidable(collidable);
   _collidableContents.push_back(collidable);
}

void ODEController::unRegisterCollidable(Transformable* collidable)
{
   if (collidable == NULL) {return;}
   _spaceWrapper->unRegisterCollidable(collidable);
   _worldWrapper->unRegisterCollidable(collidable);
   for (QVector<Transformable*>::iterator it = _collidableContents.begin();
        it != _collidableContents.end();
        ++it)
   {
      if (*it == collidable)
      {
         _collidableContents.erase(it);
         break;
      }
   }
}

ODEWorldWrap* ODEController::getWorldWrapper() const
{
   return _worldWrapper.get();
}

ODESpaceWrap* ODEController::getSpaceWrapper() const
{
   return _spaceWrapper.get();
}

void ODEController::setGravity(const osg::Vec3& gravity) const
{
   if (_worldWrapper.valid())
   {
      _worldWrapper->setGravity(gravity);
   }
}

osg::Vec3 ODEController::getGravity() const
{
   if (_worldWrapper.valid())
   {
      return _worldWrapper->getGravity();
   }
   else
   {
      return osg::Vec3(0.f, 0.f, 0.f);
   }
}

dSpaceID ODEController::getSpaceID() const
{
   if (_spaceWrapper.valid())
   {
      return _spaceWrapper->getSpaceID();
   }
   else
   {
      return 0;
   }
}

dWorldID ODEController::getWorldID() const
{
   if (_worldWrapper.valid())
   {
      return _worldWrapper->getWorldID();
   }
   else
   {
      return 0;
   }
}

dJointGroupID ODEController::getContactJointGroupID() const
{
   if (_spaceWrapper.valid())
   {
      return _spaceWrapper->getContactJoinGroupID();
   }
   else
   {
      return 0;
   }
}

void ODEController::setUserCollisionCallback(dNearCallback* func, void* data) const
{
   if (_spaceWrapper.valid())
   {
      _spaceWrapper->setUserCollisionCallback(func, data);
   }
}

dNearCallback* ODEController::getUserCollisionCallback() const
{
   return _spaceWrapper.valid() ? _spaceWrapper->getUserCollisionCallback() : NULL;
}

void* ODEController::getUserCollisionData()
{
   return _spaceWrapper.valid() ? _spaceWrapper->getUserCollisionData() : NULL;
}

const void* ODEController::getUserCollisionData() const
{
   return _spaceWrapper.valid() ? _spaceWrapper->getUserCollisionData() : NULL;
}

void ODEController::step(double stepSize)
{
   if (_msgSender.valid())
   {
      _msgSender->sendMessage(SYSTEM_MESSAGE_PHYSICS_STEP, &stepSize);
   }
   if (_spaceWrapper.valid()) { _spaceWrapper->collide(); }
   if (_worldWrapper.valid()) { _worldWrapper->step(stepSize); }
   if (_spaceWrapper.valid()) { _spaceWrapper->postCollide(); }
}

void ODEController::defaultCBFunc(ODESpaceWrap::CollisionData data)
{
   if (_msgSender.valid())
   {
      //have to convert to Scene::CollisionData for backward compatibility
      Scene::CollisionData scd;
	  scd.mBodies[0] = data._bodies[0];
	  scd.mBodies[1] = data._bodies[1];
	  scd.mDepth     = data._depth;
	  scd.mLocation  = data._location;
	  scd.mNormal    = data._normal;
      //if a collision took place and we have a sender pointer,
      //send out the "collision" message
      _msgSender->sendMessage(SYSTEM_MESSAGE_COLLISION, &scd);
   }
}

const QVector<Transformable*>& ODEController::getRegisteredCollidables() const
{
   return _collidableContents;
}

void ODEController::setMessageSender(Base* msgSender)
{
   _msgSender = msgSender;
}
}